%% Backlash Nonlinearity
%  adq@XJTU, Jan. 11 2022
%
%  y = nonlinear_backlash(x)
%  
%  Time variant function simulating a backlash behavior with slope of 1.
%  Require a global variable called 'delta'.
%  
%  State Description:
%  'HI' 'LO' for ramping up or down respectively.
%  'HL' 'LH' for transition from up ramping to down ramping, and vice versa.
%  
%  Pass in a cell to reset / initiate the function internal variables.
%  x_reset = {state, xlatch, xprev};
%  
function y = nonlinear_backlash(x)

	global delta
	% delta: width of
	persistent cstate xlatch xprev
	% cstate: current state, always in sync
	% xprev: previous value of x, always in sync
	% xlatch: x value when phase portrait is detected going backward
	%			only updated when latch state is used

	% initiate with given value
	if iscell(x)
		cstate = x{1};
		xlatch = x{2};
		xprev = x{3};
		return
	end

	dx = x - xprev;
	
	% state machine and xlatch value toggling
	% x' = Ax + Bu
	switch cstate

		case 'HI'
			if -delta < dx && dx < 0
				cstate = 'HL';
				xlatch = xprev;
			elseif dx <= -delta
				cstate = 'LO';
			end

		case 'HL'
			if x >= xlatch
				cstate = 'HI';
			elseif x <= xlatch - delta
				cstate = 'LO';
			end

		case 'LO'
			if delta > dx && dx > 0
				cstate = 'LH';
				xlatch = xprev;
			elseif dx >= delta
				cstate = 'HI';
			end

		case 'LH'
			if x <= xlatch
				cstate = 'LO';
			elseif x >= xlatch + delta
				cstate = 'HI';
			end

		otherwise
			error('Unknown state of backlash.');
	end

	% output value assigning
	% y = Cx + Du
	switch cstate
		case 'LO'
			y = x + delta/2;
		case 'HI'
			y = x - delta/2;
		case 'LH'
			y = xlatch + delta/2;
		case 'HL'
			y = xlatch - delta/2;
		otherwise
			error('Unknown state of backlash.');
	end
	
% 	i = i + 1;
% 	fprintf('%d, \tx=%.2d, %s, \ty=%.2d, xlatch=%.2d\n', i, x, cstate, y, xlatch);

	% update
	xprev = x;

end